/*
 * TestGoToPoint.java
 *
 * Created on 22 de agosto de 2007, 11:39 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.*;
import Behaviors.Schemas.Motor.*;
import Behaviors.Schemas.Perception.*;
import Robot.*;
import Utils.*;

import javaclient3.*;
import javaclient3.structures.*;

/**
 *
 * @author Alejandro Pustowka
 */
public class TestFollowLeader {

    public static void main(String[]  args)
    {

        Robot robotLeader = new Robot("Leader","localhost",6665, 0);
        Robot robotFollower = new Robot("Follower","localhost",6666, 0);

        //Interfaces de posicion de Javaclient
        Position2DInterface positionLeader = robotLeader.requestInterfacePosition2D(0,PlayerConstants.PLAYER_OPEN_MODE);
        Position2DInterface positionFollower = robotFollower.requestInterfacePosition2D(0,PlayerConstants.PLAYER_OPEN_MODE);

        //Helpers para la posicion
        Position2DHelper posHL = new Position2DHelper();
        Position2DHelper posHF = new Position2DHelper();

        //Parametros del control de rotacion de los robots
        posHL.setRotControl(0.5f,1.0f);
        posHF.setRotControl(0.5f,1.0f);
        posHL.setMaxSpeed(0.8f);
        posHF.setMaxSpeed(0.8f);



        robotFollower.runThreaded(10,0);
        robotLeader.runThreaded(10,0);

        //Posicion inicial de los robots.
        positionLeader.setOdometry(new PlayerPose(-4,-5f,(float)Math.toRadians(45.0f)));
        positionFollower.setOdometry(new PlayerPose(-7,-4,(float)Math.toRadians(0)));


        Vector2d point = new Vector2d(-1,-1,Vector2d.MODE_XY);
        
        NxPSDetectLeader detectLeader = new NxPSDetectLeader("detectLeader",positionLeader);
        NxMSFollowLeader followLeader = new NxMSFollowLeader("followLeader",positionFollower, detectLeader, 2, 0.5f);
        NxMSGoToPointNew goTo = new NxMSGoToPointNew("goTo",positionLeader,point,4,0.2f,NxMSGoToPoint.MODE_EXPONENTIAL);
        
        CooperativeArbitratorNew arbitratorLeader = new CooperativeArbitratorNew("aL", goTo, true);
        CooperativeArbitratorNew arbitratorFollower = new CooperativeArbitratorNew("aF", followLeader, true);


        positionLeader.setControlMode(1);
        while(true)
        {
            Vector2d totalL = arbitratorLeader.Value();
            Vector2d totalF = arbitratorFollower.Value();


            PlayerPose totalLP = totalL.toPlayerPose();
            PlayerPose totalFP = totalF.toPlayerPose();

            posHL.goTo(totalLP, positionLeader);
            posHF.goTo(totalFP, positionFollower);

            try
            {
            Thread.sleep(50);
            }
            catch(Exception e){}
        }
               
    
    }
    

}
